#include "Process.h"
using namespace std;
using namespace cv;


 __forceinline __m256i AnchorComponent(const int16_t* src, size_t step, const __m256i& current, const __m256i& threshold, const __m256i& mask)
{
    __m256i last = _mm256_srli_epi16(_mm256_loadu_si256((__m256i*)(src - step)), 1);
    __m256i next = _mm256_srli_epi16(_mm256_loadu_si256((__m256i*)(src + step)), 1);

    //return _mm256_or_si256(_mm256_cmpgt_epi16(threshold, _mm256_sub_epi16(current, last)),
    //    _mm256_cmpgt_epi16(threshold, _mm256_sub_epi16(current, next)));

    return _mm256_andnot_si256(_mm256_or_si256(_mm256_cmpgt_epi16(threshold, _mm256_sub_epi16(current, last)),
        _mm256_cmpgt_epi16(threshold, _mm256_sub_epi16(current, next))), mask);
}
__forceinline __m256i Anchor(const int16_t* src, size_t stride, const __m256i& threshold)
{
    __m256i _src = _mm256_loadu_si256((__m256i*)src);
    __m256i direction = _mm256_and_si256(_src, K16_0001);
    __m256i magnitude = _mm256_srli_epi16(_src, 1);//当前16位转换8位
    __m256i vertical = AnchorComponent(src, 1, magnitude, threshold, _mm256_cmpeq_epi16(direction, K16_0001));
    __m256i horizontal = AnchorComponent(src, stride, magnitude, threshold, _mm256_cmpeq_epi16(direction, K_ZERO));
    return _mm256_andnot_si256(_mm256_cmpeq_epi16(magnitude, K_ZERO), _mm256_and_si256(_mm256_or_si256(vertical, horizontal), K16_00FF));
}
__forceinline void Anchor(const int16_t* src, size_t stride, const __m256i& threshold, uint8_t* dst)
{
    __m256i lo = Anchor(src, stride, threshold);
    __m256i hi = Anchor(src + 16, stride, threshold);

    //16-8
    //__m256i lo = _mm256_srli_epi16(_mm256_loadu_si256((__m256i*)(src)), 1);
    //__m256i hi = _mm256_srli_epi16(_mm256_loadu_si256((__m256i*)(src + 16)), 1);

    _mm256_storeu_si256((__m256i*)dst, PackI16ToU8(lo, hi));
}




void ContourAnchor(cv::Mat& img, cv::Mat* Dis,uchar threshold)
{
    //判断输入地址与输出地址是否相同
    if (img.size != Dis->size || CV_8UC1 != Dis->type())
    {
        *Dis = Mat(img.size(), CV_8UC1);
    }

    static int A = 32;//AVX2
    size_t bodyWidth = AlignHi(img.cols, A) - A;
    __m256i _threshold = _mm256_set1_epi16(threshold);

    uchar* dst = Dis->ptr<uchar>(0);
    memset(dst, 0, img.cols);
    memset(Dis->ptr<uchar>(img.rows - 1), 0, img.cols);

    size_t srcStride = img.step.p[0]/2;
    short* src = img.ptr<short>(0);
    for (size_t row = 1; row < img.rows - 1; row+=1)
    {
        dst = Dis->ptr<uchar>(row);
        src = img.ptr<short>(row);
        dst[0] = 0;
        Anchor(src + 1, srcStride, _threshold, dst + 1);
        for (size_t col = A; col < bodyWidth; col += A)
            Anchor(src + col, srcStride, _threshold, dst + col);
        Anchor(src + img.cols - A - 1, srcStride, _threshold, dst + img.cols - A - 1);
        dst[img.cols - 1] = 0;
    }

}

